#include <chrono>
#include <functional>
#include <memory>
#include <cstring>
#include <cstdint>

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "std_msgs/msg/int8.hpp"
#include "std_msgs/msg/int16.hpp"

#include "usb_can.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;

float cmd_data[8] = {};
float gimbal_angle[2] = {};
int16_t robot_hp = 400;
int8_t game_state = 0;

class ControllerModule : public rclcpp::Node
{
  public:
    ControllerModule()
    : Node("controller_module")
    {
      can.usb_set("/dev/ttyACM0", 2000000);
      can.usb_open();
      subscription_cmd = this->create_subscription<geometry_msgs::msg::Twist>(
      "move_cmd", 10, std::bind(&ControllerModule::topic_callback, this, _1));
      publisher_hp = this->create_publisher<std_msgs::msg::Int16>("robot_hp", 10);
      publisher_state = this->create_publisher<std_msgs::msg::Int8>("robot_state", 10);
      timer_ = this->create_wall_timer(
      100ms, std::bind(&ControllerModule::timer_callback, this));
    }

  private:
    usb_can can;

    void timer_callback()
    {
      uint8_t read_rawdata[8];
      int16_t chassis_speed[4] = {};
      chassis_speed[0] = - cosf(gimbal_angle[0]) * cmd_data[1] - sinf(gimbal_angle[0]) * cmd_data[0];
      chassis_speed[1] = - sinf(gimbal_angle[0]) * cmd_data[1] + cosf(gimbal_angle[0]) * cmd_data[0];
      int read_id = 0;
      can.data_write((unsigned char*)chassis_speed, 0x111, 8);
      //can.data_read(read_rawdata, &read_id);
      if(read_id == 0x124)
      {
        memcpy(gimbal_angle, read_rawdata, 8);
      }
      else if(read_id == 0x151)
      {
        auto msg_state = std_msgs::msg::Int8();
        auto msg_hp = std_msgs::msg::Int16();
        robot_hp = (int16_t)read_rawdata[3] || (int16_t)(read_rawdata[4] << 8);
        game_state = read_rawdata[5];
        msg_hp.data = robot_hp;
        msg_state.data = game_state;
        publisher_hp->publish(msg_hp);
        publisher_state->publish(msg_state);
      }
    }
    rclcpp::TimerBase::SharedPtr timer_;

    void topic_callback(const geometry_msgs::msg::Twist & msg) const
    {
      cmd_data[0] = msg.linear.x * 100;
      cmd_data[1] = msg.linear.y * 100;
      cmd_data[2] = msg.angular.z * 100;
      cmd_data[3] = 10000;
      RCLCPP_INFO(this->get_logger(), "I heard: '%f'", msg.linear.x);
    }
    rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr publisher_state;
    rclcpp::Publisher<std_msgs::msg::Int16>::SharedPtr publisher_hp;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_cmd;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ControllerModule>());
  rclcpp::shutdown();
  return 0;
}